// Fill out your copyright notice in the Description page of Project Settings.

// This file contains the helper functions to generate road geometry including the road base mesh, lane-dividing line, pavement signs and etc.
#pragma once
#include <glm/glm.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <vector>
#include <stdint.h>
#include <map>
#include <unordered_map>
#include <string>
#include <memory>
#include "ODGlobal.h"

namespace rg {

	using namespace glm;
	class Setting
	{
	public:
		static bool leftHanded ;//default is right hand
	};
	struct RefPoint 
	{
		vec2 P;   // position
		vec2 T;   // normalized tangent
		double S; // distance along reference line
		float E;  // elevation

		vec3 to_xyz() const { return vec3(P, E); }
		mat3 get_rotation() const;
		RefPoint compute_offset(float offset) const;
		RefPoint lerp(const RefPoint& pt, float a) const;
	};

	// This structure is usually used as render vertex of road base geometry
	struct RoadVertex
	{
		vec3 pos;
		vec3 tangent;
		vec3 normal;
	};

	enum SubGeomType
	{
		kRoadBase,
		kRoadMarkYellow,
		kRoadMarkWhite,
		kRoadStopLine,
		kRoadCrossWalk,
		kNonDriving
	};

	struct TriangleGroup
	{
		std::vector<uint32_t> indices;
		SubGeomType type;
	};
	typedef std::vector<uint32_t> TriangleIndices;
	struct RoadDrawData
	{
		std::vector<RoadVertex> vertices;
		std::vector<TriangleGroup> groups;
	};

	struct LaneOffset 
	{
		double s;
		double a, b, c, d;
		double compute_width(double s) const;
	};

	struct Elevation
	{
		double soffset;
		double a, b, c, d;
		double compute_elevation(double s) const;
	};

	struct ElevationProfile
	{
		std::vector<Elevation> elevations;
		double compute_elevation(double s) const;
	};

	struct LaneWidth
	{
		double soffset;
		double a, b, c, d;

		LaneWidth() { soffset = 0, a = 2.0; b = 0; c = 0; d = 0; }
		LaneWidth(double _soffset, double _a, double _b, double _c, double _d) { soffset = _soffset, a = _a; b = _b; c = _c; d = _d; }
		double compute_width(double s) const;
	};

	enum RoadMarkType
	{
		kSolid = 0,
		kBroken = 1,
		kSolidSolid = 2,
		kBrokenBroken = 3,
		kSolidBroken = 4,
		kBrokenSolid = 5,


		kTypeMask = 0x0000ffff
	};

	enum RoadMarkColor
	{
		kYellow = 0x00010000,
		kWhite  = 0x00020000,

		kColorMask = 0xffff0000
	};

	struct RoadMark
	{
		double soffset;
		float width;
		int type;
	};

	struct Lane
	{
		int laneID;
		std::vector<LaneWidth> widths;
		bool using_border;
		bool is_driving;
		std::vector<RoadMark> roadMarks;

		double compute_width(double s) const;
	};

	struct LaneSection
	{
		double s;
		std::vector<Lane> centerLanes;
		std::vector<Lane> leftLanes;
		std::vector<Lane> rightLanes;

		Lane& add_left();
		Lane& add_right();
	};

	struct ReferenceLine
	{
		std::vector<RefPoint> samples;
		std::vector<RoadMark> roadMarks;

		double length() const;
		RefPoint computeRefPointAt(double s, size_t* startIdx = nullptr) const;
		size_t computeStartIdx(double s) const;
		bool isValid() const;
        // Build: Optimize ref line
		static void optimizeRefLineSamples(float threshold, std::vector<RefPoint>& line);
		void generateRoadMarkGeom(RoadDrawData& drawData) const;
		ReferenceLine computeOffsetRefLine(float offset) const;
	};
	class RoadObjectRecord
	{
	public:
		float s{ 0.0 };
		float t{ 0.0 };
		float pitch{ 0.0 };
		float hdg{ 0.0 };
		float roll{ 0.0 };
	};
	class StopLine : public RoadObjectRecord
	{
	public:
		std::vector<glm::vec3> outline; //already converted to local coordinate
		std::vector<glm::vec3> outlineoffset; //not converted
		float width{0.3f};
	};

	class  CrossWalk : public RoadObjectRecord
	{
	public:
		std::vector<glm::vec3> outline; //outline of the crosswalk
		float crossWalkGap{0.2f}; //gap between lines
		float crossWalkWidth{0.4f}; //line width
	};

	struct LaneDivRefLine : public ReferenceLine
	{
		bool isDriving = true;
		int laneID;
		std::vector<RefPoint> laneMidSamples;
	};

	struct LaneCap
	{
		int laneIdx;
		RefPoint lanePoint; //add one point to this lane refline
		RefPoint lanePoint2; //the lane refline with which makes the lane

	};
	void SerializeLaneSections(std::vector<uint8_t>& byteArray, const std::vector<LaneSection>& laneSections);
	size_t DeserializeLaneSections(void* pSrcData, std::vector<LaneSection>& laneSections);

	void SerializeStopLines(std::vector<uint8_t>& byteArray, const std::vector<StopLine>& stopLines);
	size_t DeserializeStopLines(void* pSrcData, std::vector<StopLine>& stopLines);

	void SerializeCrossWalks(std::vector<uint8_t>& byteArray, const std::vector<CrossWalk>& stopLines);
	size_t DeserializeCrossWalks(void* pSrcData, std::vector<CrossWalk>& stopLines);

	class ROAD_ENGINE_API RoadGeometry
	{
	public:
		// Check if the offset lane reference line has rewinding curve and try to remove it.
		bool mCheckRewind = false;
		bool mOptimizeGoemetry = true;
		bool mStitchConnection = false;
		int RoadId;
		int JunctionId;

		glm::dvec3 mLocalOrigin;
		std::vector<LaneOffset> mLaneOffsets;
		std::vector<LaneSection> mLaneSections;
		ElevationProfile mElevationProfile;
		std::vector<StopLine> mStopLines;
		std::vector<CrossWalk> mCrossWalks;
		typedef std::shared_ptr<RoadGeometry> Ptr;
		typedef std::weak_ptr<RoadGeometry> WeakPtr;

		typedef std::map<int, LaneDivRefLine> SectionLaneDivLines;
		std::vector<SectionLaneDivLines> mCachedLaneSectionDivLines;
		RoadDrawData mCachedMeshDrawData;

		std::vector<LaneCap> mStartCaps;
		std::vector<LaneCap> mEndCaps;

		glm::vec3 mCachedBBoxMin;
		glm::vec3 mCachedBBoxMax;

		float mMaxRoadWidth{0.0};
		ReferenceLine mRoadRefLine;

	public:
		RoadGeometry();
		virtual ~RoadGeometry() {}

		void Initialize(ReferenceLine& refLine, float sampThreshold);
		RefPoint SampleLane(double s, int laneIdx) const;
		
		// Call this function do finalize the geometry editing and compute the derived/cached data(e.g. bounding box).
		void Finalize();
        
        void CalcuBBox(); // Calculate bounding box

		std::vector<uint8_t> Serialize() const;
		size_t Deserialize(void* pSrc);

		bool IsValid() const;
		void GenerateLaneLines();

	protected:
		
		float mSampleThreshold = 0;
		bool mHasValidRoadRefLine = false;

		void GenerateRoadMesh();
		void AddLaneDivingLines(const LaneSection& section, const LaneSection* pNextSection);
		void HandleRoadMarks(const LaneSection& section, const Lane& lane, ReferenceLine& refLine);
		
	};

} // namespace rg